#include <robot/robot.h>
#include <robot/pacohead/cheadclient.h>
#include <buola/app/ccmdline.h>

using namespace buola;
using namespace buola::robot;

static CCmdLineFlag sDoReset("doreset",L"reset the head joints");

int main(int pArgC,char **pArgs)
{
    buola_init(pArgC,pArgs);
    
    try
    {
        pacohead::CHeadClient lClient;

        if(cmd_line().ArgCount())
        {
            lClient.Init(blackbox::CClientProxy(cmd_line().Arg(0))["head"]);
        }
        else
        {
            lClient.Init();
        }

        if(cmd_line().IsSet(sDoReset))
        {
            lClient.DoReset();
            return buola_finish();
        }

        while(true)
        {
            mat::CVec3d lGoalP;
            mat::C3DRotation lGoalR;

            msg_info() << "enter character 'p' followed by x y z or 'o' followed by y p r\n";

            char lChar;

            do
            {
                gIn >> lChar;
            } while(lChar!='p'&&lChar!='o'&&lChar!='h'&&lChar!='i'&&lChar!='j'
                  &&lChar!='q'&&lChar!='s'&&lChar!='z'&&lChar!='c');

            if(lChar=='p')
            {
                gIn >> lGoalP.x() >> lGoalP.y() >> lGoalP.z();

                if(norm(lGoalP)<180.0)
                    msg_info() << "Too small vector (x,y,z), please try again!\n";
                else
                {
                    lClient.SetGoal(lGoalP);
                    msg_info() << "Sent position " << lGoalP << "to HeadControlServer\n";
                }
            }
            else if(lChar=='i')
            {
                std::vector<double> lJoints;

                lClient.GetPose(lJoints);

                msg_info() << "joints=";
                for(int i=0;i<7;i++)
                {
                    if(i) msg_info() << ',';
                    msg_info() << lJoints[i];
                }
                msg_info() << "\n";
            }
            else if(lChar=='j')
            {
                int lJ;
                double lValue;
                std::cin >> lJ >> lValue;
                lClient.SetGoal(lJ,deg2rad(lValue));
            }
            else if(lChar=='q')
            {
                break;
            }
            else if(lChar=='z')
            {
                lClient.Stop();
            }
            else
            {
                msg_info() << "unknown option\n";
            }
        }
    }
    catch(std::exception &pE)
    {
        fatal_error(pE);
    }

    return buola_finish();
}
